{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PreintegratedImuMeasurements\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegratedImuMeasurements.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `PreintegratedImuMeasurements` class (often abbreviated as PIM) is the standard container in GTSAM for accumulating high-rate IMU measurements (accelerometer and gyroscope) between two keyframes or time steps ($t_i$ and $t_j$). It performs *preintegration*, effectively summarizing the relative motion $(\\Delta R_{ij}, \\Delta p_{ij}, \\Delta v_{ij})$ predicted by the IMU measurements, while accounting for sensor noise and a *fixed estimate* of the IMU bias (`biasHat_`).\n",
    "\n",
    "This preintegrated summary allows IMU information to be incorporated into a factor graph as a single factor (`ImuFactor` or `ImuFactor2`) between states at $t_i$ and $t_j$, avoiding the need to add every single IMU measurement to the graph. It also stores the 9x9 covariance matrix (`preintMeasCov_`) associated with the uncertainty of the preintegrated state change.\n",
    "\n",
    "It inherits from an underlying implementation (`ManifoldPreintegration` or `TangentPreintegration`, selected at compile time) which defines the specific mathematical approach used for integration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Background\n",
    "\n",
    "The core idea is to integrate the IMU kinematic equations *relative* to the state at time $t_i$, using the bias estimate `biasHat_` held within the class. The exact integration formulas depend on the chosen implementation (manifold or tangent space), but conceptually:\n",
    "\n",
    "$$ \\Delta R_{ij} = \\prod_{k=i}^{j-1} \\text{Exp}((\\omega_k - b_{g, est}) \\Delta t) $$ \n",
    "$$ \\Delta v_{ij} = \\sum_{k=i}^{j-1} \\Delta R_{ik} (a_k - b_{a, est}) \\Delta t $$ \n",
    "$$ \\Delta p_{ij} = \\sum_{k=i}^{j-1} [ \\Delta v_{ik} \\Delta t + \\frac{1}{2} \\Delta R_{ik} (a_k - b_{a, est}) \\Delta t^2 ] $$ \n",
    "\n",
    "where $\\omega_k$ and $a_k$ are the measurements at step k, $b_{g, est}$ and $b_{a, est}$ are the components of `biasHat_`, and $\\Delta R_{ik}$ is the preintegrated rotation from $i$ to $k$.\n",
    "\n",
    "Crucially, the class also propagates the covariance of these $\\Delta$ terms based on the IMU noise specified in `PreintegrationParams`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Constructor**: `PreintegratedImuMeasurements(params, biasHat)`: Creates an instance, requiring shared `PreintegrationParams` and the initial bias estimate (`biasHat_`) used for integration.\n",
    "- **`integrateMeasurement(measuredAcc, measuredOmega, dt)`**: Adds a single IMU measurement pair and time delta, updating the internal preintegrated state and covariance.\n",
    "- **`resetIntegration()`**: Resets the accumulated measurements and time to zero, keeping the `biasHat_`.\n",
    "- **`resetIntegrationAndSetBias(newBiasHat)`**: Resets integration and updates the internal `biasHat_`.\n",
    "- **Accessors**: `deltaTij()`, `deltaRij()`, `deltaPij()`, `deltaVij()`, `preintMeasCov()`, `biasHat()`. These return the accumulated values.\n",
    "- **`predict(state_i, current_bias)`**: Predicts the state at time $t_j$ given the state at $t_i$ and the *current best estimate* of the bias (which might differ from `biasHat_`). This function applies the necessary first-order corrections for the change in bias.\n",
    "- **`biasCorrectedDelta(current_bias)`**: Returns the 9D tangent-space representation of the preintegrated measurement, corrected for the difference between `current_bias` and `biasHat_`.\n",
    "- **`computeError(state_i, state_j, current_bias)`**: Calculates the 9D error between the preintegrated prediction (using `current_bias`) and the actual state change (`state_i` to `state_j`). This is the core calculation used within `ImuFactor::evaluateError`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Create parameters, create the PIM object with an initial bias estimate, integrate measurements, and potentially use `predict`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Total integration time: 0.09999999999999999\n",
      "Delta R:\n",
      " [[ 9.99992775e-01 -3.10098211e-03 -2.19859941e-03]\n",
      " [ 3.09900212e-03  9.99994790e-01 -9.03407707e-04]\n",
      " [ 2.20138940e-03  8.96587715e-04  9.99997175e-01]]\n",
      "Delta P: [ 0.00047953  0.00106289 -0.04859943]\n",
      "Delta V: [ 0.00993257  0.02140713 -0.97198182]\n",
      "Bias Hat Used:\n",
      "acc =  0.01 -0.01  0.02 gyro =  0.001  0.002 -0.001\n",
      "Preintegration Covariance (9x9):\n",
      " [[ 0.01  0.   -0.   -0.    0.    0.   -0.    0.    0.  ]\n",
      " [ 0.    0.01  0.   -0.   -0.   -0.   -0.   -0.   -0.  ]\n",
      " [-0.    0.    0.01 -0.    0.   -0.   -0.    0.   -0.  ]\n",
      " [-0.   -0.   -0.    0.   -0.    0.    0.05 -0.    0.  ]\n",
      " [ 0.   -0.    0.   -0.    0.    0.   -0.    0.05  0.  ]\n",
      " [ 0.   -0.   -0.    0.    0.    0.    0.    0.    0.05]\n",
      " [-0.   -0.   -0.    0.05 -0.    0.    1.   -0.    0.  ]\n",
      " [ 0.   -0.    0.   -0.    0.05  0.   -0.    1.    0.  ]\n",
      " [ 0.   -0.   -0.    0.    0.    0.05  0.    0.    1.  ]]\n",
      "\n",
      "Predicted State:\n",
      "R: [\n",
      "\t0.999993, -0.00310098, -0.0021986;\n",
      "\t0.003099, 0.999995, -0.000903408;\n",
      "\t0.00220139, 0.000896588, 0.999997\n",
      "]\n",
      "p: 0.000479535  0.00106289  -0.0976494\n",
      "v: 0.00993257  0.0214071   -1.95298\n"
     ]
    }
   ],
   "source": [
    "from gtsam import PreintegrationParams, PreintegratedImuMeasurements, NavState\n",
    "from gtsam.imuBias import ConstantBias\n",
    "import numpy as np\n",
    "\n",
    "# 1. Create Parameters (as in PreintegrationParams example)\n",
    "params = PreintegrationParams.MakeSharedU(9.81)\n",
    "accel_noise_sigma = 0.1\n",
    "gyro_noise_sigma = 0.01\n",
    "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
    "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
    "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
    "\n",
    "# 2. Define the bias estimate used for preintegration\n",
    "initial_bias_acc = np.array([0.01, -0.01, 0.02])\n",
    "initial_bias_gyro = np.array([0.001, 0.002, -0.001])\n",
    "bias_hat = ConstantBias(initial_bias_acc, initial_bias_gyro)\n",
    "\n",
    "# 3. Create the PreintegratedImuMeasurements object\n",
    "pim = PreintegratedImuMeasurements(params, bias_hat)\n",
    "\n",
    "# 4. Integrate measurements (example loop)\n",
    "dt = 0.01 # 100 Hz\n",
    "num_measurements = 10\n",
    "acc_meas = np.array([0.1, 0.2, -9.7]) # Example measurement (sensor frame)\n",
    "gyro_meas = np.array([0.01, -0.02, 0.03]) # Example measurement (sensor frame)\n",
    "\n",
    "for _ in range(num_measurements):\n",
    "    pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
    "\n",
    "# 5. Inspect the results\n",
    "print(\"Total integration time:\", pim.deltaTij())\n",
    "print(\"Delta R:\\n\", pim.deltaRij().matrix())\n",
    "print(\"Delta P:\", pim.deltaPij())\n",
    "print(\"Delta V:\", pim.deltaVij())\n",
    "print(\"Bias Hat Used:\")\n",
    "pim.biasHat().print()\n",
    "print(\"Preintegration Covariance (9x9):\\n\", np.round(1000*pim.preintMeasCov(),2))\n",
    "\n",
    "# 6. Example Prediction (requires initial state)\n",
    "initial_state = NavState() # Default: Identity rotation, zero pos/vel\n",
    "current_best_bias = bias_hat # Assume bias estimate hasn't changed yet\n",
    "predicted_state = pim.predict(initial_state, current_best_bias)\n",
    "print(\"\\nPredicted State:\")\n",
    "predicted_state.print()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `pim` object is then passed to the `ImuFactor` or `ImuFactor2` constructor."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [ImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.h) (Contains PIM definition)\n",
    "- [ImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.cpp)\n",
    "- [ManifoldPreintegration.h/.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldPreintegration.h) (One possible implementation)\n",
    "- [TangentPreintegration.h/.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/TangentPreintegration.h) (Another possible implementation)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
